import numpy as np
import queue

directions = [(1, 0), (-1, 0), (0, 1), (0, -1)]


def getMHDDistance(tmpX, tmpY, endx, endY):
    return np.abs(tmpX - endx) + np.abs(tmpY - endY)


def findShortestPath(adjacency_matrix, start, end):
    node_num = len(adjacency_matrix)
    # 用于判断当前节点是否被使用了
    node_flag = np.full((node_num, node_num), False)
    priority_queue = queue.PriorityQueue(node_num*node_num)
    cost_so_far = {}  # 记录到目前为止到达每个节点的最低成本
    visited = {}
    # node  node长度
    priority_queue.put((0, start, 0))
    while priority_queue.not_empty:
        futureLen, current_node, current_len = priority_queue.get()
        node_flag[current_node] = True
        x, y = current_node
        if (x, y) == end:
            break
        for nx, ny in directions:
            tmp_x, tmp_y = x + nx, y + ny
            if 0 <= tmp_x < node_num and 0 <= tmp_y < node_num and adjacency_matrix[tmp_x][tmp_y] != 1 and not \
            node_flag[tmp_x, tmp_y]:
                # 计算得到 实际长度
                new_len = current_len + 1
                if (tmp_x, tmp_y) not in cost_so_far or cost_so_far.get((tmp_x, tmp_y)) > new_len:
                    cost_so_far[(tmp_x, tmp_y)] = new_len
                    new_future_len = current_len + getMHDDistance(tmp_x, tmp_y, end[0], end[1])
                    priority_queue.put((new_future_len, (tmp_x, tmp_y), new_len))
                    visited[(tmp_x, tmp_y)] = current_node
    if (x, y) == end:
        path = []
        while (x, y) != start:
            path.append((x, y))
            x, y = visited[(x, y)]
        path.append(start)
        return path[::-1]
    else:
        return None


if __name__ == '__main__':
    node_num = 7
    adjacency_matrix = np.zeros((node_num, node_num))
    adjacency_matrix[1, 1] = 1
    adjacency_matrix[1, 2] = 1
    adjacency_matrix[1, 3] = 1
    adjacency_matrix[1, 4] = 1
    adjacency_matrix[1, 5] = 1
    adjacency_matrix[2, 5] = 1
    adjacency_matrix[3, 5] = 1
    adjacency_matrix[4, 5] = 1
    adjacency_matrix[5, 5] = 1
    adjacency_matrix[5, 4] = 1
    adjacency_matrix[5, 3] = 1
    adjacency_matrix[5, 2] = 1
    adjacency_matrix[5, 1] = 1
    path = findShortestPath(adjacency_matrix, (3, 2), (4, 6))
    print(path)
